/*
 * mod_motor_software_protection.c
 *
 *  Created on: 2025 Apr 10
 *      Author: SFLY
 */
#include "hal_motor_protector.h"


void Hal_Motor_Protector_Init(xt_hal_Motor_Protector_t *motor_protector)
{
    motor_protector->motor_vbus_monitor.AbnormalHighVbus = ABNORMAL_HIGH_VBUS;
    motor_protector->motor_vbus_monitor.AbnormalLowVbus = ABNORMAL_LOW_VBUS;
    motor_protector->motor_vbus_monitor.AbnormalTime = MOTOR_SYS_PWM_SWITCHING_FREQ_HZ*ABNORMAL_TIMEUS*0.000001f;
    motor_protector->motor_vbus_monitor.OvpVbusThreshold = OVP_VBUS_THRESHOLD;
    motor_protector->motor_vbus_monitor.OvpTime = MOTOR_SYS_PWM_SWITCHING_FREQ_HZ*OVP_VBUS_TIMEUS*0.000001f;
    motor_protector->motor_vbus_monitor.UvpVbusThreshold = UVP_VBUS_THRESHOLD;
    motor_protector->motor_vbus_monitor.UvpTime = MOTOR_SYS_PWM_SWITCHING_FREQ_HZ*UVP_VBUS_TIMEUS*0.000001f;
    motor_protector->motor_vbus_monitor.ErrClrTime = MOTOR_SYS_PWM_SWITCHING_FREQ_HZ*ERR_CLEAR_TIMEUS*0.000001f;
    motor_protector->motor_vbus_monitor.AbnormalCnt = 0;
    motor_protector->motor_vbus_monitor.OvpCnt = 0;
    motor_protector->motor_vbus_monitor.UvpCnt = 0;
    motor_protector->motor_current_monitor.OvpCurrentThreshold = OVP_CURRENT_THRESHOLD;
    motor_protector->motor_current_monitor.OvpTime = MOTOR_SYS_PWM_SWITCHING_FREQ_HZ*OVP_CURRENT_TIMEUS*0.000001f;
    motor_protector->motor_current_monitor.ErrClrTime = MOTOR_SYS_PWM_SWITCHING_FREQ_HZ*ERR_CLEAR_TIMEUS*0.000001f;
    motor_protector->motor_current_monitor.OvpCnt = 0;
    motor_protector->motor_temp_monitor.OvpTempThreshold = OVP_TEMP_THRESHOLD;
    motor_protector->motor_temp_monitor.OvpTime = MOTOR_SYS_PWM_SWITCHING_FREQ_HZ*OVP_TEMP_TIMEUS*0.000001f;
    motor_protector->motor_temp_monitor.ErrClrTime = MOTOR_SYS_PWM_SWITCHING_FREQ_HZ*ERR_CLEAR_TIMEUS*0.000001f;
    motor_protector->motor_temp_monitor.OvpCnt = 0;
    motor_protector->motor_vbus_monitor_update = Hal_Motor_VBus_Monitor_Update;
    motor_protector->motor_current_monitor_update = Hal_Motor_Current_Monitor_Update;
    motor_protector->motor_temp_monitor_update = Hal_Motor_Temp_Monitor_Update;
}


void Hal_Motor_VBus_Monitor_Update(float VBus,void *motor_protector_ptr)
{
    xt_hal_Motor_Protector_t* motor_protector = (xt_hal_Motor_Protector_t*)motor_protector_ptr;
    /** abnormal DC bus voltage protection ************************************/
    if ((motor_protector->ErrorFlag & ERR_VDC_ABNORM) == 0U) {
        if ((VBus >= motor_protector->motor_vbus_monitor.AbnormalHighVbus) || (VBus <= motor_protector->motor_vbus_monitor.AbnormalLowVbus)) {
            motor_protector->motor_vbus_monitor.AbnormalCnt++;
            if (motor_protector->motor_vbus_monitor.AbnormalCnt >= motor_protector->motor_vbus_monitor.AbnormalTime) {
                //持续过压，失能EPWM
                Init_EnMotorPwm(FALSE);
                motor_protector->ErrorFlag |= ERR_VDC_ABNORM;
                motor_protector->motor_vbus_monitor.AbnormalCnt = motor_protector->motor_vbus_monitor.ErrClrTime;
            }
        } else {
            motor_protector->motor_vbus_monitor.AbnormalCnt = 0;
        }
    } else {
        /** start counting for clearing error if no error exist ***************/
        if ((VBus <=  motor_protector->motor_vbus_monitor.AbnormalHighVbus) && (VBus >=  motor_protector->motor_vbus_monitor.AbnormalLowVbus)) {
            motor_protector->motor_vbus_monitor.AbnormalCnt--;
            if (motor_protector->motor_vbus_monitor.AbnormalCnt <= 0) {
                motor_protector->ErrorFlag &= (~ERR_VDC_ABNORM);
            }
        } else {
            motor_protector->motor_vbus_monitor.AbnormalCnt = motor_protector->motor_vbus_monitor.ErrClrTime;
        }
    }

    /** over DC bus voltage protection ****************************************/
    if ((motor_protector->ErrorFlag & ERR_VDC_OV) == 0U) {
        if (VBus >= motor_protector->motor_vbus_monitor.OvpVbusThreshold) {
            motor_protector->motor_vbus_monitor.OvpCnt++;
            if (motor_protector->motor_vbus_monitor.OvpCnt >= motor_protector->motor_vbus_monitor.OvpTime) {
                motor_protector->ErrorFlag|= ERR_VDC_OV;
                Init_EnMotorPwm(FALSE);
                motor_protector->motor_vbus_monitor.OvpCnt = motor_protector->motor_vbus_monitor.ErrClrTime;
            }
        } else {
            motor_protector->motor_vbus_monitor.OvpCnt = 0;
        }
    } else { /** clear over voltage error if DC bus voltage less than OV limit *********/
        if (VBus <= motor_protector->motor_vbus_monitor.OvpVbusThreshold) {
            motor_protector->motor_vbus_monitor.OvpCnt--;
            if (motor_protector->motor_vbus_monitor.OvpCnt <= 0) {
                motor_protector->ErrorFlag &= (~ERR_VDC_OV);
            }
        } else {
            motor_protector->motor_vbus_monitor.OvpCnt =motor_protector->motor_vbus_monitor.ErrClrTime;
        }
    }

    /** under DC bus voltage protection ***************************************/
    if ((motor_protector->ErrorFlag & ERR_VDC_UV) == 0U) {
        if (VBus <= motor_protector->motor_vbus_monitor.UvpVbusThreshold) {
            motor_protector->motor_vbus_monitor.UvpCnt++;
            if (motor_protector->motor_vbus_monitor.UvpCnt >= motor_protector->motor_vbus_monitor.UvpTime) {
                motor_protector->ErrorFlag |= ERR_VDC_UV;
                Init_EnMotorPwm(FALSE);
                motor_protector->motor_vbus_monitor.UvpCnt = motor_protector->motor_vbus_monitor.ErrClrTime;
            }
        } else {
            motor_protector->motor_vbus_monitor.UvpCnt = 0;
        }
    } else { /** clear under voltage error if DC bus voltage greater than UV limit *****/
        if (VBus >= motor_protector->motor_vbus_monitor.UvpVbusThreshold) {
            motor_protector->motor_vbus_monitor.UvpCnt--;
            if (motor_protector->motor_vbus_monitor.UvpCnt <= 0) {
                motor_protector->ErrorFlag &= (~ERR_VDC_UV);
            }
        } else {
            motor_protector->motor_vbus_monitor.UvpCnt = motor_protector->motor_vbus_monitor.ErrClrTime;
        }
    }
}

void Hal_Motor_Current_Monitor_Update(float Iu,float Iv,float Iw,void *motor_protector_ptr)
{

    xt_hal_Motor_Protector_t* motor_protector = (xt_hal_Motor_Protector_t*)motor_protector_ptr;
        float DeltaU = fabsf(Iu) - motor_protector->motor_current_monitor.OvpCurrentThreshold;
        float DeltaV = fabsf(Iv) -  motor_protector->motor_current_monitor.OvpCurrentThreshold;
        float DeltaW = fabsf(Iw) -  motor_protector->motor_current_monitor.OvpCurrentThreshold;
        /** over current protection of peak value *********************************/
        if ((motor_protector->ErrorFlag & ERR_OC_PEAK) == 0U) {
            if ((DeltaU >= 0) || (DeltaV >= 0) || (DeltaW >= 0)) {
                motor_protector->motor_current_monitor.OvpCnt++;
                if (motor_protector->motor_current_monitor.OvpCnt >= (int32_t) motor_protector->motor_current_monitor.OvpTime) {
                    motor_protector->ErrorFlag |= ERR_OC_PEAK;
                    motor_protector->motor_current_monitor.OvpCnt = motor_protector->motor_current_monitor.ErrClrTime;
                }
            } else {
                motor_protector->motor_current_monitor.OvpCnt = 0;
            }
        } else { /** clear over current error if DC bus voltage less than OV limit *********/
            if ((DeltaU <= 0) || (DeltaV <= 0) || (DeltaW <= 0)) {
                motor_protector->motor_current_monitor.OvpCnt--;
                if (motor_protector->motor_current_monitor.OvpCnt <= 0) {
                    motor_protector->ErrorFlag &= (~ERR_OC_PEAK);
                }
            } else {
                motor_protector->motor_current_monitor.OvpCnt= motor_protector->motor_current_monitor.ErrClrTime;
            }
        }
}

void Hal_Motor_Temp_Monitor_Update(float Temp,void *motor_protector_ptr)
{
    xt_hal_Motor_Protector_t* motor_protector = (xt_hal_Motor_Protector_t*)motor_protector_ptr;
    /** over DC bus voltage protection ****************************************/
     if ((motor_protector->ErrorFlag & ERR_MOTOR_OT) == 0U) {
         if (Temp >= motor_protector->motor_temp_monitor.OvpTempThreshold) {
             motor_protector->motor_temp_monitor.OvpCnt++;
             if (motor_protector->motor_temp_monitor.OvpCnt >= motor_protector->motor_temp_monitor.OvpTime) {
                 motor_protector->ErrorFlag|= ERR_MOTOR_OT;
                 Init_EnMotorPwm(FALSE);
                 motor_protector->motor_temp_monitor.OvpCnt =motor_protector->motor_temp_monitor.ErrClrTime;
             }
         } else {
             motor_protector->motor_temp_monitor.OvpCnt = 0;
         }
     } else { /** clear over voltage error if DC bus voltage less than OV limit *********/
         if (Temp <= motor_protector->motor_temp_monitor.OvpTempThreshold) {
             motor_protector->motor_temp_monitor.OvpCnt--;
             if (motor_protector->motor_temp_monitor.OvpCnt <= 0) {
                 motor_protector->ErrorFlag &= (~ERR_MOTOR_OT);
             }
         } else {
             motor_protector->motor_temp_monitor.OvpCnt =motor_protector->motor_temp_monitor.ErrClrTime;
         }
     }
}
